// Copyright 2022 Chen Jun
// Licensed under the MIT License.

#ifndef ARMOR_DETECTOR__PNP_SOLVER_HPP_
#define ARMOR_DETECTOR__PNP_SOLVER_HPP_

#include <geometry_msgs/msg/point.hpp>
#include <opencv2/core.hpp>

// STD
#include <array>
#include <vector>

#include "armor_detector/armor.hpp"
/*PnPSolver 类的作用是为装甲板提供姿态估计和距离测量的功能，以便后续的目标追踪和击打准备。
它使用 PnP（Perspective-n-Point）算法来计算装甲板的姿态（旋转向量和平移向量），并提供计算装甲板中心点与图像中心点距离的功能。*/
namespace rm_auto_aim
{
class PnPSolver
{
public:
  // 使用摄像机矩阵和畸变系数初始化 PnPSolver 对象
  PnPSolver(
    const std::array<double, 9> & camera_matrix,
    const std::vector<double> & distortion_coefficients);

  /*Get 3d position使用 PnP（Perspective-n-Point）算法计算装甲板的姿态估计（旋转向量和平移向量）
  该函数接受 Armor 对象作为输入，并返回解算得到的旋转向量 rvec 和平移向量 tvec*/ 
  bool solvePnP(const Armor & armor, cv::Mat & rvec, cv::Mat & tvec);

  // Calculate the distance between armor center and image center计算装甲板中心点与图像中心点的距离
  float calculateDistanceToCenter(const cv::Point2f & image_point);

private:
  cv::Mat camera_matrix_;//相机矩阵
  cv::Mat dist_coeffs_;//畸变系数

  // Unit: mmSMALL_ARMOR_WIDTH、SMALL_ARMOR_HEIGHT、LARGE_ARMOR_WIDTH 和 LARGE_ARMOR_HEIGHT：小装甲板和大装甲板的宽度和高度，以毫米为单位。
  static constexpr float SMALL_ARMOR_WIDTH = 135;
  static constexpr float SMALL_ARMOR_HEIGHT = 55;
  static constexpr float LARGE_ARMOR_WIDTH = 225;
  static constexpr float LARGE_ARMOR_HEIGHT = 55;

  // Four vertices of armor in 3d小装甲板和大装甲板的四个顶点在三维空间中的坐标。
  std::vector<cv::Point3f> small_armor_points_;
  std::vector<cv::Point3f> large_armor_points_;
};

}  // namespace rm_auto_aim

#endif  // ARMOR_DETECTOR__PNP_SOLVER_HPP_
